#include "led.h"
#include "usart.h"
#include "speed_reading.h"
#include "sys.h"
#include "PWM.h"
#include "vofa+.h"

unsigned char Flag_Motor_Reverse[4]={0};
long Current_Inpluse[4];
long Relative_Counter[4];
u16 Speed_Time_Counter;
u16 Target_Routine_ms=200;
u16 Target_Inpluse[4]={0,0,0,0};
u16 Default_Counter_Value=65536/2;
u16 MainMotor_Speed[MainMotor_SpeedLevel]={0,60,120,180,240};

 
void  Speed_Reading_Init(void)
{
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3|RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB, ENABLE);//使能PC端口时钟
    GPIO_InitTypeDef  GPIO_InitStructure;//设置GPIO为TIM的时钟输入引脚
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;    //端口配置
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
    GPIO_Init(GPIOA, &GPIO_InitStructure);                          //根据设定参数初始化GPIOC      
    GPIO_Init(GPIOB, &GPIO_InitStructure);          

    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);//使能PC端口时钟
    
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0|GPIO_Pin_12;    //端口配置
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; //浮空输入
    GPIO_Init(GPIOA, &GPIO_InitStructure);                          //根据设定参数初始化GPIOC      
    TIM_DeInit(TIM1);
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;//设置定时器参数
    TIM_TimeBaseStructure.TIM_Period =Default_Counter_Value;//自动重装载值
    TIM_TimeBaseStructure.TIM_Prescaler = 0; //定时器分频
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;//向上计数模式
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
    TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure);
    TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
    TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);

    TIM_ClearITPendingBit(TIM3,TIM_IT_Update);//清除溢出中断标志位
    TIM_ITConfig(TIM3,TIM_IT_Update,DISABLE);//使能定时器更新中断
    TIM_ClearITPendingBit(TIM4,TIM_IT_Update);//清除溢出中断标志位
    TIM_ITConfig(TIM4,TIM_IT_Update,DISABLE);//使能定时器更新中断
    TIM_ClearITPendingBit(TIM2,TIM_IT_Update);//清除溢出中断标志位
    TIM_ITConfig(TIM2,TIM_IT_Update,DISABLE);//使能定时器更新中断
    TIM_ClearITPendingBit(TIM1,TIM_IT_Update);//清除溢出中断标志位
    TIM_ITConfig(TIM1,TIM_IT_Update,DISABLE);//使能定时器更新中断

    NVIC_InitTypeDef NVIC_InitStructure;//设置中断优先级
    NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn|TIM2_IRQn|TIM3_IRQn|TIM4_IRQn;  
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;  
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 3;  
    NVIC_InitStructure.NVIC_IRQChannelCmd = DISABLE;
    NVIC_Init(&NVIC_InitStructure);
 
    TIM_TIxExternalClockConfig(TIM3,TIM_TIxExternalCLK1Source_TI1,TIM_ICPolarity_Rising,5);//设置输入滤波和边沿检测
    TIM_SetCounter(TIM3,Default_Counter_Value/2);//计数器清零
    TIM_Cmd(TIM3, ENABLE);//使能TIMx外设
    TIM_TIxExternalClockConfig(TIM4,TIM_TIxExternalCLK1Source_TI1,TIM_ICPolarity_Rising,5);//设置输入滤波和边沿检测
    TIM_SetCounter(TIM4,Default_Counter_Value/2);//计数器清零
    TIM_Cmd(TIM4, ENABLE);//使能TIMx外设
    TIM_TIxExternalClockConfig(TIM2,TIM_TIxExternalCLK1Source_TI1,TIM_ICPolarity_Rising,5);//设置输入滤波和边沿检测
    TIM_SetCounter(TIM2,Default_Counter_Value/2);//计数器清零
    TIM_Cmd(TIM2, ENABLE);//使能TIMx外设
    TIM_ITRxExternalClockConfig(TIM1,TIM_TS_ETRF);          //配置外部触发，否则不会计数
    TIM_ETRClockMode2Config(TIM1, TIM_ExtTRGPSC_OFF, TIM_ExtTRGPolarity_NonInverted, 6);
    TIM_SetCounter(TIM1,Default_Counter_Value/2);//计数器清零
    TIM_Cmd(TIM1, ENABLE);//使能TIMx外设
}
void TIM3_IRQHandler(void) //TIM3中断
{ 
    if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET) //检查 TIM8更新中断发生与否
    {
        TIM_ClearITPendingBit(TIM3, TIM_IT_Update ); //清除 TIM8更新中断标志
    } 
}
void TIM4_IRQHandler(void) //TIM3中断
{ 
    if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET) //检查 TIM8更新中断发生与否
    {
        TIM_ClearITPendingBit(TIM4, TIM_IT_Update ); //清除 TIM8更新中断标志
    } 
}
void TIM2_IRQHandler(void) //TIM3中断
{ 
    if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET) //检查 TIM8更新中断发生与否
    {
        TIM_ClearITPendingBit(TIM2, TIM_IT_Update ); //清除 TIM8更新中断标志
    } 
}
void TIM1_IRQHandler(void) //TIM3中断
{ 
    if (TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET) //检查 TIM8更新中断发生与否
    {
        TIM_ClearITPendingBit(TIM1, TIM_IT_Update ); //清除 TIM8更新中断标志
    } 
}

void Speed_Reading_Control(void)
{
    if(Speed_Time_Counter >= Target_Routine_ms)
	{
		
		Relative_Counter[0] = TIM_GetCounter(TIM3); 
		TIM3 -> CNT=(Default_Counter_Value/2);
        Current_Inpluse[0]=Relative_Counter[0]-(Default_Counter_Value/2);
        Relative_Counter[1] = TIM_GetCounter(TIM4); 
		TIM4 -> CNT=(Default_Counter_Value/2);
        Current_Inpluse[1]=Relative_Counter[1]-(Default_Counter_Value/2);

        Relative_Counter[2] = TIM_GetCounter(TIM1); 
        TIM1 -> CNT=(Default_Counter_Value/2);
        Current_Inpluse[2]=Relative_Counter[2]-(Default_Counter_Value/2);
        Relative_Counter[3] = TIM_GetCounter(TIM2); 
        TIM2 -> CNT=(Default_Counter_Value/2);
        Current_Inpluse[3]=Relative_Counter[3]-(Default_Counter_Value/2);

        for (int i=0;i<4;i++)
        {
            if(Current_Inpluse[i]<0)
            {
                Current_Inpluse[i]*=-1;
                Flag_Motor_Reverse[i]=1;
            }
            else
            {
                Flag_Motor_Reverse[i]=0;
            }
            PID_Speed_Calc(i);
            Print_Data[i]=(float)PWM1_value[i];
        }
        
		TIM_SetCompare1(TIM8,PWM1_value[0]);
        TIM_SetCompare2(TIM8,PWM1_value[1]);
        TIM_SetCompare3(TIM8,PWM1_value[2]);
        TIM_SetCompare4(TIM8,PWM1_value[3]);
		Speed_Time_Counter=0;
		
	}
}
//速度调节函数，防止单边锁定自旋的时候速度不统一导致的切回前后前进时候速度暴涨的问题。
//Orientation_Data 方向信息变量，传入板间通讯的方向控制参数，控制各个轮子有无速度；
//Speed_Data：速度信息变量，直接控制所有轮子的速度。
void MainMotor_SpeedAdjust(u8 Orientation_Data,u8 Speed_Data)
{
    u16 AdjustSpeed;
    
    if((Speed_Data<MainMotor_SpeedLevel))
    {
        AdjustSpeed=MainMotor_Speed[Speed_Data];
    }
    else
    {
        return;
    }
    
    if((Orientation_Data&0x08)||(Orientation_Data&0x04))//左边电机启动
    {
        Target_Inpluse[1]=AdjustSpeed;
        Target_Inpluse[3]=AdjustSpeed;
    }
    else
    {
        Target_Inpluse[1]=0;
        Target_Inpluse[3]=0;
    }

    if((Orientation_Data&0x02)||(Orientation_Data&0x01))//右边电机启动
    {
        Target_Inpluse[0]=AdjustSpeed;
        Target_Inpluse[2]=AdjustSpeed;
    }
    else
    {
        Target_Inpluse[0]=0;
        Target_Inpluse[2]=0;
    }
}
